(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[     71873,       2030]
NotebookOptionsPosition[     71185,       2005]
NotebookOutlinePosition[     71543,       2021]
CellTagsIndexPosition[     71500,       2018]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "NAO", "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"use", " ", "rT", " ", "instead", " ", "of", " ", "LT"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", "initialization", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/output_v2/models/NAO\>\"", "]"}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"A_", "?", "MatrixQ"}], "]"}], ":=", 
     RowBox[{"ParallelTable", "[", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{"A", "\[LeftDoubleBracket]", 
         RowBox[{"i", ",", "j"}], "\[RightDoubleBracket]"}], "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", 
         RowBox[{
          RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "1", 
          "\[RightDoubleBracket]"}]}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"j", ",", 
         RowBox[{
          RowBox[{"Dimensions", "[", "A", "]"}], "\[LeftDoubleBracket]", "2", 
          "\[RightDoubleBracket]"}]}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", 
      RowBox[{"A_", "?", "VectorQ"}], "]"}], ":=", 
     RowBox[{"ParallelTable", "[", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{"A", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", 
         RowBox[{"Length", "[", "A", "]"}]}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"ParallelSimplify", "[", "A_", "]"}], ":=", 
     RowBox[{"Simplify", "[", "A", "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.525028795242095*^9, 3.525028798347091*^9}, {
   3.525029058469722*^9, 3.525029081601416*^9}, {3.527366972613077*^9, 
   3.527367010701981*^9}, {3.527367079389505*^9, 3.527367091508872*^9}, {
   3.5273674161254797`*^9, 3.52736742619272*^9}, 3.52737014603018*^9, {
   3.5276127491893167`*^9, 3.527612751321732*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"robotInfo", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
        "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", 
         "mh", ",", "mt", ",", "mc", ",", "mf", ",", "g"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{
        "380", ",", "273.60", ",", "0", ",", "282.37", ",", "129.66", ",", 
         "5.27", ",", "746.56", ",", "940.74", ",", "213.79", ",", "0", ",", 
         "9810"}], "}"}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"constsubs", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Lc", "\[Rule]", 
        FractionBox["5137", "50000"]}], ",", 
       RowBox[{"Lt", "\[Rule]", 
        FractionBox["1", "10"]}], ",", 
       RowBox[{"LT", "\[Rule]", 
        FractionBox["30218105", "209827159"]}], ",", 
       RowBox[{"ha", "\[Rule]", 
        FractionBox["4511", "100000"]}], ",", 
       RowBox[{"hf", "\[Rule]", 
        FractionBox["37116100", "1208051301"]}], ",", 
       RowBox[{"lf", "\[Rule]", 
        FractionBox["357", "25000"]}], ",", 
       RowBox[{"lh", "\[Rule]", 
        FractionBox["6", "125"]}], ",", 
       RowBox[{"lt", "\[Rule]", 
        FractionBox["261", "2500"]}], ",", 
       RowBox[{"mh", "\[Rule]", 
        FractionBox["130763", "50000"]}], ",", 
       RowBox[{"mt", "\[Rule]", 
        FractionBox["3333", "6250"]}], ",", 
       RowBox[{"mc", "\[Rule]", 
        FractionBox["29951", "50000"]}], ",", 
       RowBox[{"mf", "\[Rule]", 
        FractionBox["7549", "25000"]}], ",", 
       RowBox[{"rc", "\[Rule]", 
        FractionBox["6322731", "325193729"]}], ",", 
       RowBox[{"rt", "\[Rule]", 
        FractionBox["66471778", "1134333797"]}], ",", 
       RowBox[{"rT", "\[Rule]", 
        FractionBox["30218105", "209827159"]}], ",", 
       RowBox[{"g", "\[Rule]", 
        FractionBox["981", "100"]}]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"mm", "=", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.",
       "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"statesubs", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", "i", "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{
           SuperscriptBox[
            SubscriptBox["\[Theta]", "i"], "\[Prime]",
            MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
          RowBox[{"x", "[", 
           RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"define", " ", "inertia", " ", "matrices"}], "*)"}], 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
       RowBox[{
        RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
        " ", 
        RowBox[{"DiagonalMatrix", "[", 
         RowBox[{"{", 
          RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}],
    "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      RowBox[{"1", "-", "2"}], ":", " ", "left"}], ",", " ", 
     RowBox[{
      RowBox[{"4", "-", "5"}], ":", " ", "right"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"2.524082978466`*^6", "10713.891532`", 
             RowBox[{"-", "126075.629642`"}]},
            {"10713.891532`", "2.986642586756`*^6", "71009.473708`"},
            {
             RowBox[{"-", "126075.629642`"}], "71009.473708`", 
             "884479.151346`"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        FormBox[
         TagBox[
          RowBox[{"(", "\[NoBreak]", GridBox[{
             {"3.769899366786`*^6", "883.233008`", "111261.284918`"},
             {"883.233008`", "3.838716842066`*^6", "72389.846622`"},
             {"111261.284918`", "72389.846622`", "436115.120736`"}
            },
            
            GridBoxAlignment->{
             "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
              "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
            GridBoxSpacings->{"Columns" -> {
                Offset[0.27999999999999997`], {
                 Offset[0.7]}, 
                Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
                Offset[0.2], {
                 Offset[0.4]}, 
                Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
          Function[BoxForm`e$, 
           MatrixForm[BoxForm`e$]]],
         TraditionalForm], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
     RowBox[{"Rationalize", "[", 
      RowBox[{
       RowBox[{
        TagBox[
         RowBox[{"(", "\[NoBreak]", GridBox[{
            {"8.642632927562952`*^7", 
             RowBox[{"-", "516556.06079885364`"}], 
             RowBox[{"-", "5.922561441486226`*^6"}]},
            {
             RowBox[{"-", "516556.060798854`"}], "9.021952557224008`*^7", 
             RowBox[{"-", "1.5119885900291622`*^7"}]},
            {
             RowBox[{"-", "5.922561441486225`*^6"}], 
             RowBox[{"-", "1.511988590029162`*^7"}], 
             "3.1728770928063706`*^7"}
           },
           
           GridBoxAlignment->{
            "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
             "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
           GridBoxSpacings->{"Columns" -> {
               Offset[0.27999999999999997`], {
                Offset[0.7]}, 
               Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
               Offset[0.2], {
                Offset[0.4]}, 
               Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
         Function[BoxForm`e$, 
          MatrixForm[BoxForm`e$]]], "/", "1000000000"}], ",", 
       RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "2"], "*", 
      RowBox[{"(", GridBox[{
         {"1", 
          RowBox[{"-", "1"}], "1"},
         {
          RowBox[{"-", "1"}], "1", 
          RowBox[{"-", "1"}]},
         {"1", 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalI]", "1"], "*", 
      RowBox[{"(", GridBox[{
         {"1", 
          RowBox[{"-", "1"}], "1"},
         {
          RowBox[{"-", "1"}], "1", 
          RowBox[{"-", "1"}]},
         {"1", 
          RowBox[{"-", "1"}], "1"}
        }], ")"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"Needs", "[", "\"\<Units`\>\"", "]"}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "Put", " ", "rotational", " ", "inertia", " ", "into", " ", "generalized",
      " ", "manipulator", " ", "inertia", " ", "and", " ", "add", " ", 
     "rotor", " ", "and", " ", "gearhead", " ", "inertia"}], "*)"}], 
   RowBox[{
    RowBox[{"For", "[", 
     RowBox[{
      RowBox[{"i", "=", "1"}], ",", 
      RowBox[{"i", "\[LessEqual]", 
       RowBox[{"Length", "[", "mm", "]"}]}], ",", 
      RowBox[{"i", "++"}], ",", 
      RowBox[{
       SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"Join", "[", 
          RowBox[{
           SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
           RowBox[{"Table", "[", 
            RowBox[{"0", ",", 
             RowBox[{"{", "3", "}"}], ",", 
             RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
         RowBox[{"Join", "[", 
          RowBox[{
           RowBox[{"Table", "[", 
            RowBox[{"0", ",", 
             RowBox[{"{", "3", "}"}], ",", 
             RowBox[{"{", "3", "}"}]}], "]"}], ",", 
           SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", 
         "1"}], "]"}]}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"p0", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "y"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{"\[Psi]", "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "x"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "y"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox[
          SubscriptBox["p", "z"], "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
       RowBox[{
        RowBox[{
         SuperscriptBox["\[Psi]", "\[Prime]",
          MultilineFunction->None], "[", "t", "]"}], "\[Rule]", "0"}]}], 
      "}"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.5212045686835737`*^9, 3.521204606993765*^9}, {
   3.521204700067089*^9, 3.521204780986717*^9}, {3.5212145569110236`*^9, 
   3.5212145580280876`*^9}, {3.5212214210366507`*^9, 
   3.5212215159440784`*^9}, {3.521221549532*^9, 3.521221581995857*^9}, {
   3.5212225825050826`*^9, 3.5212225872403536`*^9}, {3.5212910875692406`*^9, 
   3.5212910900933847`*^9}, {3.5212911767343407`*^9, 
   3.5212911783804345`*^9}, {3.521291778077735*^9, 3.5212918161939154`*^9}, {
   3.521291985634607*^9, 3.521291991076918*^9}, {3.5212922081883364`*^9, 
   3.5212922159737816`*^9}, {3.5214849738620605`*^9, 3.521484978075301*^9}, {
   3.5214860083742313`*^9, 3.5214860164166913`*^9}, {3.522244545041182*^9, 
   3.522244546126244*^9}, {3.522244593734967*^9, 3.5222446610148153`*^9}, {
   3.522245182408637*^9, 3.5222451980385313`*^9}, {3.5222452480633926`*^9, 
   3.5222452884067*^9}, {3.5222453190144506`*^9, 3.522245320543538*^9}, {
   3.5222453895034823`*^9, 3.522245390398534*^9}, {3.5222454580864053`*^9, 
   3.5222454737583017`*^9}, {3.522508873288*^9, 3.522508878436*^9}, {
   3.52251692506606*^9, 3.5225169259961133`*^9}, {3.5226866514206*^9, 
   3.5226866515376*^9}, {3.5226870047536*^9, 3.5226870474105997`*^9}, {
   3.5226870857056*^9, 3.5226870879856*^9}, {3.5226871451626*^9, 
   3.5226871878106003`*^9}, 3.5230284498409986`*^9, {3.527367143201222*^9, 
   3.5273671538939753`*^9}, 3.5273671849756613`*^9, 3.527367344578813*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", 
            FractionBox["43", "10000"]}], ",", "0", ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"-", 
            FractionBox["43", "10000"]}], ",", "0", ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "j"], "[", "\[Theta]", "]"}], "=", 
       RowBox[{"Simplify", "@", 
        RowBox[{"(", 
         RowBox[{"ForwardKinematics", "@@", 
          RowBox[{"Append", "[", 
           RowBox[{
            RowBox[{"Table", "[", 
             RowBox[{
              RowBox[{"{", 
               RowBox[{
                SubscriptBox["\[Xi]", 
                 RowBox[{"If", "[", 
                  RowBox[{
                   RowBox[{"i", "\[LessEqual]", "j"}], ",", 
                   RowBox[{"Symbol", "[", 
                    RowBox[{"\"\<q\>\"", "<>", 
                    RowBox[{"ToString", "[", "i", "]"}]}], "]"}], ",", "0"}], 
                  "]"}]], ",", 
                RowBox[{
                 SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "}"}], ",", 
              RowBox[{"{", 
               RowBox[{"i", ",", 
                RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}], ",", 
            RowBox[{
             SubscriptBox["g", 
              SubscriptBox["sl", "j"]], "[", "0", "]"}]}], "]"}]}], ")"}]}]}],
       ",", 
      RowBox[{"{", 
       RowBox[{"j", ",", 
        RowBox[{"Length", "[", "mm", "]"}]}], "}"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"change", " ", "LT", " ", "to", " ", "rT"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, {3.5273673526577663`*^9, 
   3.5273673678729773`*^9}, 3.527367510176667*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "aniplot", "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
       "]"}], "]"}]}], ";"}], "\n", "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}]}]}]], "Input",\

 CellChangeTimes->{{3.513875953621905*^9, 3.513876010689337*^9}, {
   3.513876043248599*^9, 3.51387610532577*^9}, {3.513876169675573*^9, 
   3.513876183706996*^9}, {3.513887655923167*^9, 3.513887658548081*^9}, {
   3.513888227999695*^9, 3.513888239050703*^9}, {3.5158587000371203`*^9, 
   3.51585871925922*^9}, {3.5158693371802073`*^9, 3.5158693660608597`*^9}, {
   3.5158696092147675`*^9, 3.5158696155961323`*^9}, {3.515886157784644*^9, 
   3.515886172869871*^9}, {3.516708254867365*^9, 3.5167082553383923`*^9}, {
   3.527367518307618*^9, 3.5273675270140142`*^9}, 3.5273677031466503`*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
    "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
        ".923"}]}], "}"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["gdot", "nsf"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"D", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "[", 
          RowBox[{"[", 
           RowBox[{"1", ",", "4"}], "]"}], "]"}], "/.", "p0"}], ",", "t"}], 
       "]"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.513015087195842*^9, 3.513015090509893*^9}, {
   3.5167083039851747`*^9, 3.5167083075683794`*^9}, 3.516708384001751*^9, {
   3.5273677230197906`*^9, 3.527367759954082*^9}, {3.527367799151667*^9, 
   3.527367810348826*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], "\n", 
  RowBox[{"(*", 
   RowBox[{"1", " ", "Hip", " ", "Position"}], "*)"}], "\[IndentingNewLine]", 
  
  RowBox[{
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"non", "-", 
     RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["xp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], 
         "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["zp", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], 
         "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"stcomslope", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], ")"}], "/", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}], ")"}]}], "/.", "p0"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"\[Delta]stcomslopePre", "=", 
      RowBox[{"ParallelSimplify", "[", 
       RowBox[{"Normal", "[", 
        RowBox[{"Series", "@@", 
         RowBox[{"Prepend", "[", 
          RowBox[{
           RowBox[{"Table", "[", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], ",", "0", ",",
                "1"}], "}"}], ",", 
             RowBox[{"{", 
              RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
           "stcomslope"}], "]"}]}], "]"}], "]"}]}], ";", "\n", 
     RowBox[{"\[Delta]stcomslope", "=", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], "5"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"Collect", "[", 
          RowBox[{"\[Delta]stcomslopePre", ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "]"}], "/.", 
         "\[InvisibleSpace]", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{"#1", "\[Rule]", "0"}], "&"}], ")"}], "/@", 
          RowBox[{"Complement", "[", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", 
            RowBox[{"{", 
             RowBox[{
              SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}]}], 
           "]"}]}]}], ")"}]}]}], ";"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[Delta]stcomslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"stcomslope", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"nstcomslope", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["xp", "COM"]}], ")"}], "/", 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{
           SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
          "\[LeftDoubleBracket]", 
          RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
         SubscriptBox["zp", "COM"]}], ")"}]}], "/.", "p0"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"\[Delta]nstcomslopePre", "=", 
      RowBox[{"ParallelSimplify", "[", 
       RowBox[{"Normal", "[", 
        RowBox[{"Series", "@@", 
         RowBox[{"Prepend", "[", 
          RowBox[{
           RowBox[{"Table", "[", 
            RowBox[{
             RowBox[{"{", 
              RowBox[{
               RowBox[{
                SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], ",", "0", ",",
                "1"}], "}"}], ",", 
             RowBox[{"{", 
              RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
           "nstcomslope"}], " ", "]"}]}], "]"}], "]"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"\[Delta]nstcomslope", "=", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], "5"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{"Collect", "[", 
          RowBox[{"\[Delta]nstcomslopePre", ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "i"], "[", "t", "]"}]}], "]"}], "/.", 
         "\[InvisibleSpace]", 
         RowBox[{
          RowBox[{"(", 
           RowBox[{
            RowBox[{"#1", "\[Rule]", "0"}], "&"}], ")"}], "/@", 
          RowBox[{"Complement", "[", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", 
            RowBox[{"{", 
             RowBox[{
              SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}]}], 
           "]"}]}]}], ")"}]}]}], ";"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[Delta]nstcomslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstcomslope", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"theta4", " ", "=", " ", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], 
     "\[IndentingNewLine]", 
     RowBox[{"TorsoHipAngle", "=", " ", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{
       RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
       RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\[IndentingNewLine]", 
     RowBox[{"nstorso", " ", "=", " ", 
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{
       RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
       RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], 
     "\[IndentingNewLine]", 
     RowBox[{"LinearNStorso", "=", 
      RowBox[{"Linearize", "[", 
       RowBox[{
        RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], 
     "\[IndentingNewLine]", 
     RowBox[{"storso", " ", "=", " ", 
      FractionBox[
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";", "\n", 
     RowBox[{"(*", 
      RowBox[{
      "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
      "*)"}], "\n", 
     RowBox[{"LinearStorso", "=", 
      RowBox[{"Linearize", "[", 
       RowBox[{
        RowBox[{"storso", " ", "/.", "p0"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";", 
     "\[IndentingNewLine]", 
     RowBox[{"(*", 
      RowBox[{"7", " ", "COM"}], "*)"}], "\[IndentingNewLine]", 
     RowBox[{
      SubscriptBox["p", "COM"], "=", 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           UnderoverscriptBox["\[Sum]", 
            RowBox[{"i", "=", "1"}], 
            RowBox[{"Length", "[", "mm", "]"}]], 
           RowBox[{
            RowBox[{
            "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
            RowBox[{
             RowBox[{
              RowBox[{
               SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
              "\[LeftDoubleBracket]", 
              RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{"2", "*", 
                RowBox[{"(", 
                 RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}],
           "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
     ";", "\n", 
     RowBox[{"(*", 
      RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
     RowBox[{
      SubscriptBox["Linearp", "COM"], "=", " ", 
      RowBox[{"Linearize", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"Subscript", "[", 
            RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
    "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}, {
   3.527266876626479*^9, 3.5272669091021013`*^9}, 3.5272680582479362`*^9, {
   3.52726826837751*^9, 3.5272682685913153`*^9}, {3.527268323380795*^9, 
   3.52726834320557*^9}, {3.527268721025804*^9, 3.527268740044486*^9}, {
   3.527281326545467*^9, 3.527281336017424*^9}, 3.527282982629828*^9, {
   3.52736794853358*^9, 3.5273679739134893`*^9}, {3.527368012098797*^9, 
   3.5273680558134604`*^9}, {3.527368140597526*^9, 3.527368140978167*^9}, {
   3.527612631540802*^9, 3.527612690089253*^9}, {3.5276183321726723`*^9, 
   3.527618384654684*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/output_v2/models/NAO/\>\"", "<>", 
    "\"\<build_torso\>\""}], "]"}], ";"}]], "Input",
 CellChangeTimes->{{3.513876298225372*^9, 3.513876314895954*^9}, 
   3.5207161215675383`*^9, 3.527367774435699*^9, 3.5273688940470953`*^9, 
   3.527368976025791*^9, {3.5273692811108093`*^9, 3.527369283382406*^9}, {
   3.527369735661566*^9, 3.52736973625198*^9}, 3.527369922791857*^9, {
   3.527370173166801*^9, 3.527370177766226*^9}}],

Cell[BoxData[
 RowBox[{"\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<stCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"stcomslope", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltastCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"\[Delta]stcomslope", "/.", "p0"}], "/.", "constsubs"}], "/.",
        "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nstCOMFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nstcomslope", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltanstCOMFn\>\"", "]"}]}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"\[Delta]nstcomslope", "/.", "p0"}], "/.", "constsubs"}], "/.",
        "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<hipposFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"HipPos", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaHipposFn\>\"", "]"}]}], ";"}], "\n", 
   
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"LHipPos", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<nsslopeFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"nsslope", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaNSslopeFn\>\"", "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        RowBox[{"LinearNSslope", "/.", "p0"}], "/.", "constsubs"}], "/.", 
       "statesubs"}]}], "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Clear", "[", "stream", "]"}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.52717692204714*^9, 3.5271769389718723`*^9}, {
   3.527177770383316*^9, 3.5271777843116817`*^9}, 3.5273698919196157`*^9, {
   3.5276128695380917`*^9, 3.527612911880995*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
  "SetDirectory", "[", 
   "\"\</home/shu/workspace/Research/output_v2/models/NAO\>\"", "]"}], ";", 
  RowBox[{"Run", "[", "\"\<perl math2mat_output.pl\>\"", "]"}]}]], "Input",
 CellChangeTimes->{{3.527612921342675*^9, 3.5276129299037113`*^9}}],

Cell[BoxData[
 FormBox["0", TraditionalForm]], "Output",
 CellChangeTimes->{3.527612985423975*^9, 3.527613025432021*^9, 
  3.527618412788268*^9}]
}, Open  ]]
},
WindowSize->{959, 1024},
WindowMargins->{{19, Automatic}, {Automatic, -8}},
ShowSelection->True,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 2178, 54, 202, "Input"],
Cell[2726, 76, 12237, 315, 710, "Input"],
Cell[14966, 393, 23360, 691, 1063, "Input"],
Cell[38329, 1086, 5605, 144, 315, "Input"],
Cell[43937, 1232, 4760, 140, 185, "Input"],
Cell[48700, 1374, 17442, 480, 891, "Input"],
Cell[66145, 1856, 527, 10, 30, "Input"],
Cell[66675, 1868, 4042, 119, 639, "Input"],
Cell[CellGroupData[{
Cell[70742, 1991, 279, 6, 50, "Input"],
Cell[71024, 1999, 145, 3, 31, "Output"]
}, Open  ]]
}
]
*)

(* End of internal cache information *)
